#include "stm32f10x.h"                  // Device header
#include "PID.h"
#include "Encoder.h"
#include "inv_mpu.h"

void PID_Calculate(PID_t* p)
{
	assert_param(p!=NULL);
	
	p->Error1=p->Error0;
	
	p->Error0=p->Target-p->Actual;
	
	if(p->ErrorInt<500)
	{
		p->ErrorInt+=p->Error0;
	}
	else
	{
		p->ErrorInt=500;
	}
	
	p->Out=p->Kp*p->Error0+p->Ki*p->ErrorInt+p->Kd*(p->Error0-p->Error1);
	
	if(p->Out>p->OutMax)
	{
		p->Out=p->OutMax;
	}
	else if(p->Out<p->OutMin)
	{
		p->Out=p->OutMin;
	}
}
